﻿namespace EnergyGeneratorSim
{
    using System;
    using System.Diagnostics;
    using EnergyGeneratorSim.Parameters;

    public static class AdvancedMath
    {
        /// <summary>
        /// Initializes the <see cref="AdvancedMath"/> class.
        /// </summary>
        static AdvancedMath()
        {
            TwoPI = 2 * Math.PI;
        }

        /// <summary>
        /// Gets or sets the value of  2 * PI.
        /// </summary>
        /// <value>The two PI.</value>
        public static double TwoPI { get; private set; }

        public static double RadToDegree(double radAngle)
        {
            return radAngle / TwoPI * 360;
        }

        public static double DegreeToRad(double degreeAngle)
        {
            return degreeAngle / 360 * TwoPI;
        }

        public static double Fourier(double t, double Ko1, double Ko2, double Ko3, double Ko4, double Ko5)
        {         // "t" ist Variable für den Winkel, anzugeben in Grad
            double merk = 0.0;
            merk += Ko1 * Math.Sin(1.0 * t / 360.0 * 2.0 * Math.PI);
            merk += Ko2 * Math.Sin(2.0 * t / 360.0 * 2.0 * Math.PI);
            merk += Ko3 * Math.Sin(3.0 * t / 360.0 * 2.0 * Math.PI);
            merk += Ko4 * Math.Sin(4.0 * t / 360.0 * 2.0 * Math.PI);
            merk += Ko5 * Math.Sin(5.0 * t / 360.0 * 2.0 * Math.PI);
            return merk;
        }

        public static void FourierEntwicklung(SimulationParameters param, SimulationDataItem[] data, Func<SimulationDataItem, double> propertyAccessor, out double B1, out double B2, out double B3, out double B4, out double B5)
        {
            double[] PSIF = new double[data.Length];
            double QSplus, QSmitte, QSminus;
            double Qanf, Q1p, Q1m, Q2p, Q2m, Q3p, Q3m; // für B1,2,3 - Iteration
            double Q4p, Q4m, Q5p, Q5m; // für B4,5 - Iteration
            double QSminimum; // zur Minimums-Suche
            bool weiter;

            Debug.Assert(data.Length == 360);

            for (int i = 0; i < data.Length; i++)
            {
                PSIF[i] = propertyAccessor(data[i]);
            }

            B1 = 0;   // Mittelwert über erste Periode als Startwert für Grundschwingung
            for (int i = 0; i < 180; i++)
            {
                B1 += propertyAccessor(data[i]);
            }

            // Zuerst die Größenordnung von B1 abschätzen:
            B1 /= 90;

            // Jetzt B1 anpassen über die Minimierung der Abweichungsquadrate:
            do
            {
                B1 = 0.99 * B1;
                QSminus = QuadSum1(PSIF, B1);
                B1 = B1 / 0.99;
                QSmitte = QuadSum1(PSIF, B1);
                B1 = 1.01 * B1;
                QSplus = QuadSum1(PSIF, B1);
                B1 = B1 / 1.01;
                weiter = false;

                if (QSminus < QSmitte)
                {
                    B1 = 0.99 * B1;
                    weiter = true;
                }

                if (QSplus < QSmitte)
                {
                    B1 = 1.01 * B1;
                    weiter = true;
                }
                //    PrintLine("QS: ",QSminus,", ",QSmitte,", ",QSplus); 
            }
            while (!weiter);

            //PrintLine("B1-vorab : " + B1 + ",  QS = " + QSmitte);
            // Die Werte zur Kontrolle herausschreiben:
            param.AnzP = 360; param.Abstd = 1;
            for (int i = 0; i < 360; i++)      // "i" ist Laufvariable für den Winkel, anzugeben in Grad
            {
                data[i].Q = PSIF[i];
                data[i].Qp = B1 * Math.Sin(1.0 * i / 360.0 * 2.0 * Math.PI);
            }

            // Dann B1 & B2 & B3 anpassen über die Minimierung der Abweichungsquadrate:
            // Startwerte für B2 suchen:
            B2 = 0;
            B2 = +B1 / 10; QSplus = QuadSum3(B1, B2, 0, PSIF);
            B2 = -B1 / 10; QSminus = QuadSum3(B1, B2, 0, PSIF);
            if (QSplus < QSminus) B2 = +B1 / 10;
            if (QSminus < QSplus) B2 = -B1 / 10;
            // Startwerte für B3 suchen:
            B3 = 0;
            B3 = +B1 / 10; QSplus = QuadSum3(B1, B2, B3, PSIF);
            B3 = -B1 / 10; QSminus = QuadSum3(B1, B2, B3, PSIF);
            if (QSplus < QSminus) B3 = +B1 / 10;
            if (QSminus < QSplus) B3 = -B1 / 10;
            //PrintLine("AnfB1,2,3: " + B1 + " ,  " + B2 + " ,  " + B3);

            // Jetzt über Iteration die B1, B2, B3 fitten:
            do
            {
                // QuadSummen berechnen:
                Qanf = QuadSum3(B1, B2, B3, PSIF);
                Q1p = QuadSum3(B1 * 1.01, B2, B3, PSIF);
                Q1m = QuadSum3(B1 * 0.99, B2, B3, PSIF);
                Q2p = QuadSum3(B1, B2 * 1.01, B3, PSIF);
                Q2m = QuadSum3(B1, B2 * 0.99, B3, PSIF);
                Q3p = QuadSum3(B1, B2, B3 * 1.01, PSIF);
                Q3m = QuadSum3(B1, B2, B3 * 0.99, PSIF);
                // Kleinste QuadSumme suchen:
                QSminimum = Qanf;
                if (Q1p < QSminimum)
                {
                    QSminimum = Q1p;
                }

                if (Q1m < QSminimum)
                {
                    QSminimum = Q1m;
                }

                if (Q2p < QSminimum)
                {
                    QSminimum = Q2p;
                }

                if (Q2m < QSminimum)
                {
                    QSminimum = Q2m;
                }

                if (Q3p < QSminimum)
                {
                    QSminimum = Q3p;
                }

                if (Q3m < QSminimum)
                {
                    QSminimum = Q3m;
                }

                // Koeffizienten zur kleinsten QuadSumme einstellen:
                weiter = false;
                if (Q1p == QSminimum) { B1 = B1 * 1.01; weiter = true; }
                if (Q1m == QSminimum) { B1 = B1 * 0.99; weiter = true; }
                if (Q2p == QSminimum) { B2 = B2 * 1.01; weiter = true; }
                if (Q2m == QSminimum) { B2 = B2 * 0.99; weiter = true; }
                if (Q3p == QSminimum) { B3 = B3 * 1.01; weiter = true; }
                if (Q3m == QSminimum) { B3 = B3 * 0.99; weiter = true; }
                //    PrintLine("QS: ",QSminimum); 
            } while (!weiter);

            //PrintLine("Nun B1 = " + B1 + ",  B2 = " + B2 + "  B3 = " + B3);
            //PrintLine("Zugehörige Quadsum: " + QuadSum3(B1, B2, B3, PSIF));

            // Die Werte zur Kontrolle herausschreiben:
            for (int i = 0; i < 360; i++)
            {
                data[i].Qpp =
                    B1 * Math.Sin(1.0 * i / 360.0 * 2.0 * Math.PI) +
                    B2 * Math.Sin(2.0 * i / 360.0 * 2.0 * Math.PI) +
                    B3 * Math.Sin(3.0 * i / 360.0 * 2.0 * Math.PI);
            }

            // Nun will ich alle Ausreißer mit mehr als 75% Abweichung löschen:
            for (int i = 0; i < 360; i++)
            {
                if (Math.Abs(PSIF[i] - (B1 * Math.Sin(1.0 * i / 360.0 * 2.0 * Math.PI) - B2 * Math.Sin(2.0 * i / 360.0 * 2.0 * Math.PI) - B3 * Math.Sin(3.0 * i / 360.0 * 2.0 * Math.PI))) > Math.Abs(0.75 * B1))
                {
                    PSIF[i] = 0;
                }
            }

            // Dazu will nun eine Fourier-Reihe mit 5 Koeffizienten fitten:
            // Startwerte für B4 suchen:
            B4 = 0;
            B4 = +B1 / 40.0;
            QSplus = QuadSum5(B1, B2, B3, B4, 0, PSIF);
            B4 = -B1 / 40.0;
            QSminus = QuadSum5(B1, B2, B3, B4, 0, PSIF);

            if (QSplus < QSminus)
            {
                B4 = +B1 / 40.0;
            }

            if (QSminus < QSplus)
            {
                B4 = -B1 / 40.0;
            }

            // Startwerte für B5 suchen:
            B5 = 0;
            B5 = +B1 / 40.0;
            QSplus = QuadSum5(B1, B2, B3, B4, B5, PSIF);
            B5 = -B1 / 40.0;
            QSminus = QuadSum5(B1, B2, B3, B4, B5, PSIF);

            if (QSplus < QSminus)
            {
                B5 = +B1 / 10.0;
            }

            if (QSminus < QSplus)
            {
                B5 = -B1 / 10.0;
            }

            //PrintLine("Und B4,5: " + B4 + " ,  " + B5);
            //PrintLine("Anf Quadsum: " + QuadSum5(B1, B2, B3, B4, B5, PSIF));

            // Jetzt über Iteration die B1, B2, B3, B4, B5 fitten:
            do
            {
                // QuadSummen berechnen:
                Qanf = QuadSum5(B1, B2, B3, B4, B5, PSIF);
                Q1p = QuadSum5(B1 * 1.01, B2, B3, B4, B5, PSIF);
                Q1m = QuadSum5(B1 * 0.99, B2, B3, B4, B5, PSIF);
                Q2p = QuadSum5(B1, B2 * 1.01, B3, B4, B5, PSIF);
                Q2m = QuadSum5(B1, B2 * 0.99, B3, B4, B5, PSIF);
                Q3p = QuadSum5(B1, B2, B3 * 1.01, B4, B5, PSIF);
                Q3m = QuadSum5(B1, B2, B3 * 0.99, B4, B5, PSIF);
                Q4p = QuadSum5(B1, B2, B3, B4 * 1.01, B5, PSIF);
                Q4m = QuadSum5(B1, B2, B3, B4 * 0.99, B5, PSIF);
                Q5p = QuadSum5(B1, B2, B3, B4, B5 * 1.01, PSIF);
                Q5m = QuadSum5(B1, B2, B3, B4, B5 * 0.99, PSIF);
                // Kleinste QuadSumme suchen:
                QSminimum = Qanf;
                if (Q1p < QSminimum)
                    QSminimum = Q1p;
                if (Q1m < QSminimum)
                    QSminimum = Q1m;
                if (Q2p < QSminimum)
                    QSminimum = Q2p;
                if (Q2m < QSminimum)
                    QSminimum = Q2m;
                if (Q3p < QSminimum)
                    QSminimum = Q3p;
                if (Q3m < QSminimum)
                    QSminimum = Q3m;
                if (Q4p < QSminimum)
                    QSminimum = Q4p;
                if (Q4m < QSminimum)
                    QSminimum = Q4m;
                if (Q5p < QSminimum)
                    QSminimum = Q5p;
                if (Q5m < QSminimum)
                    QSminimum = Q5m;
                // Koeffizienten zur kleinsten QuadSumme einstellen:
                weiter = false;
                if (Q1p == QSminimum) { B1 = B1 * 1.01; weiter = true; }
                if (Q1m == QSminimum) { B1 = B1 * 0.99; weiter = true; }
                if (Q2p == QSminimum) { B2 = B2 * 1.01; weiter = true; }
                if (Q2m == QSminimum) { B2 = B2 * 0.99; weiter = true; }
                if (Q3p == QSminimum) { B3 = B3 * 1.01; weiter = true; }
                if (Q3m == QSminimum) { B3 = B3 * 0.99; weiter = true; }
                if (Q4p == QSminimum) { B4 = B4 * 1.01; weiter = true; }
                if (Q4m == QSminimum) { B4 = B4 * 0.99; weiter = true; }
                if (Q5p == QSminimum) { B5 = B5 * 1.01; weiter = true; }
                if (Q5m == QSminimum) { B5 = B5 * 0.99; weiter = true; }
                //    PrintLine("QS: ",QSminimum); 
            } while (!weiter);

            //PrintLine("Ergebnis: B1 = " + B1 + ",  B2 = " + B2 + "  B3 = " + B3);
            //PrintLine("          B4 = " + B4 + ",  B5 = " + B5);
            //PrintLine("Endliche Quadsum: " + QuadSum5(B1, B2, B3, B4, B5, PSIF));
            // Die Werte zur Kontrolle herausschreiben:

            for (int i = 0; i < 360; i++)
            {
                data[i].phipp = Fourier(i, B1, B2, B3, B4, B5);
            }

            //ExcelAusgabe(param, "fourier.dat", 6);
        }

        public static void SinusEntwicklung_fuer_Drehmoment(SimulationParameters param, SimulationDataItem[] data, Func<SimulationDataItem, double> propertyAccessor, out double B1, out double phase)
        {
            double[] PSIF = new double[360];
            //fin  : Text;
            double QSalt, QSneu;
            bool weiter;
            double Qanf, QB1plus, QB1minus, Qphaseplus, Qphaseminus; // für numerische Iteration
            double QSminimum; // Zur Suche des kleinsten Abweichungsquadrates. 

            Debug.Assert(data.Length == 360);

            for (int i = 0; i < data.Length; i++)
            {
                PSIF[i] = propertyAccessor(data[i]);
            }

            /*
            using (var stream = File.OpenRead("PSIF.DAT"))
            using (var fin = new StreamReader(stream))
            {
                //PrintLine("FOURIER-ENTWICKLUNG FÜR DIE SCHNELLE DREHMOMENTS-BERECHNUNG:");
                for (int i = 0; i < 360; i++)
                {
                    var text = fin.ReadLine();
                    PSIF[i] = double.Parse(text);
                }
            }*/

            B1 = 0;   // Startwert für Grundschwingung "B1" suchen
            for (int i = 0; i < 360; i++)
            {
                if (PSIF[i] > B1)
                {
                    B1 = PSIF[i];
                }
            }

            //PrintLine("Startwert von B1: " + B1);
            //Wait();
            phase = 0;
            QSalt = QuadSum2(B1, phase, PSIF);
            int jmerk = (int)(phase + 0.5); // Startwert für Grundschwingung "phase" suchen
            for (int j = 1; j < 360; j++)
            {
                phase = j;
                QSneu = QuadSum2(B1, phase, PSIF);
                if (QSneu < QSalt)
                {
                    QSalt = QSneu;
                    jmerk = j;
                }

                phase = jmerk;
            }

            //PrintLine("Startwert von phase: " + phase);
            //Wait();

            // Jetzt folgt noch eine genaue Iteration der Parameter:
            do
            {
                // QuadSummen berechnen:
                Qanf = QuadSum2(B1, phase, PSIF);
                QB1plus = QuadSum2(B1 * 1.0001, phase, PSIF);
                QB1minus = QuadSum2(B1 * 0.9999, phase, PSIF);
                Qphaseplus = QuadSum2(B1, phase * 1.0001, PSIF);
                Qphaseminus = QuadSum2(B1, phase * 0.9999, PSIF);
                // Kleinste QuadSumme suchen:
                QSminimum = Qanf;
                if (QB1plus < QSminimum) QSminimum = QB1plus;
                if (QB1minus < QSminimum) QSminimum = QB1minus;
                if (Qphaseplus < QSminimum) QSminimum = Qphaseplus;
                if (Qphaseminus < QSminimum) QSminimum = Qphaseminus;
                // Koeffizienten zur kleinsten QuadSumme einstellen:
                weiter = false;
                if (QB1plus == QSminimum) { B1 = B1 * 1.0001; weiter = true; }
                if (QB1minus == QSminimum) { B1 = B1 * 0.9999; weiter = true; }
                if (Qphaseplus == QSminimum) { phase = phase * 1.0001; weiter = true; }
                if (Qphaseminus == QSminimum) { phase = phase * 0.9999; weiter = true; }
                //PrintLine("QS: " + QSminimum);
            } while (!weiter);
            phase = phase / 360.0 * 2.0 * Math.PI; // Phase auf Radianten einstellen
        }

        private static double QuadSum1(double[] PSIF, double B1)
        {
            double merk = 0;    // "i" ist Laufvariable für den Winkel, anzugeben in Grad
            for (int i = 0; i < 360; i++)
                merk += Physics.Sqr(PSIF[i] - B1 * Math.Sin(1.0 * i / 360.0 * 2.0 * Math.PI));
            return merk;
        }

        private static double QuadSum2(double B1lok, double phaselok, double[] PSIF)
        {
            double merk = 0;    // "i" ist Laufvariable für den Winkel, anzugeben in Grad
            for (int i = 0; i < 360; i++)
            {
                merk += Physics.Sqr(PSIF[i] - B1lok * Math.Sin((i + phaselok) / 360.0 * 2.0 * Math.PI));
            }

            return merk;
        }

        private static double QuadSum3(double Koeff1, double Koeff2, double Koeff3, double[] PSIF)
        {
            double merk = 0;        // "i" ist Laufvariable für den Winkel, anzugeben in Grad
            for (int i = 0; i < 360; i++)
                merk += Physics.Sqr(
                    PSIF[i] - Koeff1 *
                    Math.Sin(1.0 * i / 360.0 * 2.0 * Math.PI) - Koeff2 *
                    Math.Sin(2.0 * i / 360.0 * 2.0 * Math.PI) - Koeff3 *
                    Math.Sin(3.0 * i / 360.0 * 2.0 * Math.PI));
            return merk;
        }

        private static double QuadSum5(double Koeff1, double Koeff2, double Koeff3, double Koeff4, double Koeff5, double[] PSIF)
        {
            double merk = 0;

            for (int i = 0; i < 360; i++)    // "i" ist Laufvariable für den Winkel, anzugeben in Grad
            {
                if (PSIF[i] != 0)
                {
                    merk += Physics.Sqr(PSIF[i] - AdvancedMath.Fourier(i, Koeff1, Koeff2, Koeff3, Koeff4, Koeff5));
                }
            }
            return merk;
        }
    }
}
